#include <iostream>
#include <cmath>
#include <stdlib.h>

/*!
 * \brief Satellite route
 *
 *  x = x0 + vx*t + ax*t*t; where t from [0...1]
 *
 **/
struct s_route
{
    double x0, y0, z0;
    double vx, vy, vz;
    double ax, ay, az;

    double r0, p0, j0;
    double vr, vp, vj;
    double ar, ap, aj;
};

struct s_attitude
{
 double r; double p; double j;
 void rotation_matrix( double* rot );
};

struct s_point3d
{
 double X; double Y; double Z;
};

struct s_point2d
{
 double x; double y;
};

void project( s_point2d* prj, s_attitude* attitude, double f, s_point3d* center, s_point3d* point );
void project_points( s_attitude* attitude, double f, s_point3d* center);

int main()
{
  s_attitude attitude;
  s_point2d  prj;
  s_point3d  center;
  s_point3d  point;
  double rot[9],f;
  std::cout << "Input roll angle\n";
  std::cin >> attitude.r;
  std::cout << "Input pitch angle\n";
  std::cin >> attitude.p;
  std::cout << "Input yaw angle\n";
  std::cin >> attitude.j;
  std::cout << "Input focal length\n";
  std::cin >> f;
  std::cout << "Input PC position Xc\n";
  std::cin >> center.X;
  std::cout << "Input PC position Yc\n";
  std::cin >> center.Y;
  std::cout << "Input PC position Zc\n";
  std::cin >> center.Z;
  project_points( &attitude, f, &center);
  return 0;
}


void s_attitude::rotation_matrix( double* rot )
{
    std::cout << "\n\nMatrica povorota\n\n";
    //Считаем матрицу поворота
    rot[0] = cos(p)*cos(j);
    rot[1] = -cos(p)*sin(j);
    rot[2] = sin(p);
    rot[3] = cos(r)*sin(j)+sin(r)*sin(p)*cos(j);
    rot[4] = cos(r)*cos(j)-sin(r)*sin(p)*sin(j);
    rot[5] = -sin(r)*cos(p);
    rot[6] = sin(r)*sin(j)-cos(r)*sin(p)*cos(j);
    rot[7] = sin(r)*cos(j)+cos(r)*sin(p)*sin(j);
    rot[8] = cos(r)*cos(p);
    std::cout << rot[0] << "  " << rot[1] << "  " << rot[2] << "\n"
             << rot[3] << "  " << rot[4] << "  " << rot[5] << "\n"
             << rot[6] << "  " << rot[7] << "  " << rot[8] << "\n";
}

void project( s_point2d* prj, s_attitude* attitude, double f, s_point3d* center, s_point3d* point )
{
    //Проецируем точку пространства на изображение
    double      rot[9];
    attitude->rotation_matrix( rot );
    prj->x=-f*(rot[0]*( point->X - center->X )+rot[3]*(point->Y - center->Y)+rot[6]*(point->Z- center->Z))/rot[2]*(point->X- center ->X)+rot[5]*(point ->Y- center->Y)+rot[8]*(point->Z- center->Z);
    prj->y=-f*(rot[1]*(point->X - center->X )+rot[4]*(point->Y - center->Y)+rot[7]*(point->Z- center->Z))/rot[2]*(point->X- center ->X)+rot[5]*(point ->Y- center->Y)+rot[8]*(point->Z- center->Z);
    std::cout << "\n   x\t       y             X="<< point->X << " Y=" << point->Y << " Z=" << point->Z << "\n";
    std::cout << prj->x << "  " << prj->y;
}

void project_points( s_attitude* attitude, double f, s_point3d* center)
{
    s_point3d p3d;
    s_point2d prj;
    //Выбираем случайные точки в пространстве XYZ (много)
    //Проецируем их на изображение
    //Выводим в консоль таблиц
    for( int i=0; i < 1000; ++i )
    {
    p3d.X=rand()%10;
    p3d.Y=rand()%10;
    p3d.Z=rand()%10;
    project(&prj, attitude, f,  center, &p3d);
    }
}
